#include <stdio.h>
#include <stdlib.h>
#include <GL/gl.h>
#include "geometry.h"
#include "assembler.h"
#include "clip.h"

void sqsSendToRaster(struct INTERNAL_VERTEX v) {
    glColor4f(v.color1[0], v.color1[1], v.color1[2], v.color1[3]);
    glVertex4f(v.normcoord[X], v.normcoord[Y], v.normcoord[Z], v.normcoord[W]);
}

void sqsInterpolate(struct INTERNAL_VERTEX *v1, struct INTERNAL_VERTEX *v2, float u1, float u2) {
    double dx = v2->normcoord[X] - v1->normcoord[X];
    double dy = v2->normcoord[Y] - v1->normcoord[Y];
    double dz = v2->normcoord[Z] - v1->normcoord[Z];
    double dw = v2->normcoord[W] - v1->normcoord[W];
    double dr = v2->color1[0] - v1->color1[0];
    double dg = v2->color1[1] - v1->color1[1];
    double db = v2->color1[2] - v1->color1[2];
    double da = v2->color1[3] - v1->color1[3];
    double dr2 = v2->color2[0] - v1->color2[0];
    double dg2 = v2->color2[1] - v1->color2[1];
    double db2 = v2->color2[2] - v1->color2[2];
    double da2 = v2->color2[3] - v1->color2[3];
    double dnx = v2->normal[X] - v1->normal[X];
    double dny = v2->normal[Y] - v1->normal[Y];
    double dnz = v2->normal[Z] - v1->normal[Z];
    double dtx = v2->texcoord1[X] - v1->texcoord1[X];
    double dty = v2->texcoord1[Y] - v1->texcoord1[Y];
    double dtz = v2->texcoord1[Z] - v1->texcoord1[Z];
    double dtw = v2->texcoord1[W] - v1->texcoord1[W];

    v2->normcoord[X] = v1->normcoord[X] + (dx * u2);
    v2->normcoord[Y] = v1->normcoord[Y] + (dy * u2);
    v2->normcoord[Z] = v1->normcoord[Z] + (dz * u2);
    v2->normcoord[W] = v1->normcoord[W] + (dw * u2);

    sqsSetClipCode(v2);

    v1->normcoord[X] = v1->normcoord[X] + (dx * u1);
    v1->normcoord[Y] = v1->normcoord[Y] + (dy * u1);
    v1->normcoord[Z] = v1->normcoord[Z] + (dz * u1);
    v1->normcoord[W] = v1->normcoord[W] + (dw * u1);

    sqsSetClipCode(v1);

    v2->color1[X] = v1->color1[X] + (dr * u2);
    v2->color1[Y] = v1->color1[Y] + (dg * u2);
    v2->color1[Z] = v1->color1[Z] + (db * u2);
    v2->color1[W] = v1->color1[W] + (da * u2);

    v1->color1[X] = v1->color1[X] + (dr * u1);
    v1->color1[Y] = v1->color1[Y] + (dg * u1);
    v1->color1[Z] = v1->color1[Z] + (db * u1);
    v1->color1[W] = v1->color1[W] + (da * u1);

    v2->color2[X] = v1->color2[X] + (dr2 * u2);
    v2->color2[Y] = v1->color2[Y] + (dg2 * u2);
    v2->color2[Z] = v1->color2[Z] + (db2 * u2);
    v2->color2[W] = v1->color2[W] + (da2 * u2);

    v1->color2[X] = v1->color2[X] + (dr2 * u1);
    v1->color2[Y] = v1->color2[Y] + (dg2 * u1);
    v1->color2[Z] = v1->color2[Z] + (db2 * u1);
    v1->color2[W] = v1->color2[W] + (da2 * u1);

    v2->normal[X] = v1->normal[X] + (dnx * u2);
    v2->normal[Y] = v1->normal[Y] + (dny * u2);
    v2->normal[Z] = v1->normal[Z] + (dnz * u2);

    v1->normal[X] = v1->normal[X] + (dnx * u1);
    v1->normal[Y] = v1->normal[Y] + (dny * u1);
    v1->normal[Z] = v1->normal[Z] + (dnz * u1);

    v2->texcoord1[X] = v1->texcoord1[X] + (dtx * u2);
    v2->texcoord1[Y] = v1->texcoord1[Y] + (dty * u2);
    v2->texcoord1[Z] = v1->texcoord1[Z] + (dtz * u2);
    v2->texcoord1[W] = v1->texcoord1[W] + (dtw * u2);

    v1->texcoord1[X] = v1->texcoord1[X] + (dtx * u1);
    v1->texcoord1[Y] = v1->texcoord1[Y] + (dty * u1);
    v1->texcoord1[Z] = v1->texcoord1[Z] + (dtz * u1);
    v1->texcoord1[W] = v1->texcoord1[W] + (dtw * u1);
}

void sqsSetClipCode(struct INTERNAL_VERTEX *v) {
    float x = v->normcoord[X];
    float y = v->normcoord[Y];
    float z = v->normcoord[Z];
    float w = v->normcoord[W];
    w = fabs(w);
    v->clipcode = (char) 0;

    if (x < -w) {//left
        v->clipcode |= 32;
    } else if (x > w) {//right
        v->clipcode |= 16;
    }

    if (y < -w) {//bottom
        v->clipcode |= 8;
    } else if (y > w) {//top
        v->clipcode |= 4;
    }

    if (z > w) {//near
        v->clipcode |= 2;
    } else if (z < -w) {//far
        v->clipcode |= 1;
    }
}

void sqsClipPoint(struct INTERNAL_VERTEX v) {
    if (v.clipcode == (char) 0) {
        glBegin(GL_POINT);
        sqsSendToRaster(v);
        glEnd();
    }
}

/*
 * Liang-Barsky
 */
void sqsClipLine(struct INTERNAL_VERTEX v1, struct INTERNAL_VERTEX v2) {
    double dx, dy, dz, dw, p[6], q[6], r[6], u1 = 0.0, u2 = 1.0;
    int k;

    if ((v1.clipcode | v2.clipcode) == (char) 0) {
        glBegin(GL_LINES);
        sqsSendToRaster(v1);
        sqsSendToRaster(v2);
        glEnd();
        return;
    }

    if ((v1.clipcode & v2.clipcode) != (char) 0) {
        return;
    }

    dx = v2.normcoord[X] - v1.normcoord[X];
    dy = v2.normcoord[Y] - v1.normcoord[Y];
    dz = v2.normcoord[Z] - v1.normcoord[Z];
    dw = v2.normcoord[W] - v1.normcoord[W];
    p[0] = -(dx + dw);
    p[1] = dx - dw;
    p[2] = -(dy + dw);
    p[3] = dy - dw;
    p[4] = -(dz + dw);
    p[5] = dz - dw;
    q[0] = v1.normcoord[X] + v1.normcoord[W];
    q[1] = -v1.normcoord[X] + v1.normcoord[W];
    q[2] = v1.normcoord[Y] + v1.normcoord[W];
    q[3] = -v1.normcoord[Y] + v1.normcoord[W];
    q[4] = v1.normcoord[Z] + v1.normcoord[W];
    q[5] = -v1.normcoord[Z] + v1.normcoord[W];

    for (k = 0; k < 6; k++) {
        if (p[k] == 0.0) {
            if (q[k] >= 0.0) {
                glBegin(GL_LINES);
                sqsSendToRaster(v1);
                sqsSendToRaster(v2);
                glEnd();
            }
            return;
        }
    }

    for (k = 0; k < 6; k++) {
        if (p[k] < 0.0) {
            r[k] = q[k] / p[k];
            if (u1 < r[k]) {
                u1 = r[k];
            }
        }
    }
    for (k = 0; k < 6; k++) {
        if (p[k] > 0.0) {
            r[k] = q[k] / p[k];
            if (u2 > r[k]) {
                u2 = r[k];
            }
        }
    }
    if (u1 > u2) {
        return;
    }
    if (u1 < 0.0){
        u1 = 0.0;
    }
    if (u2 > 1.0){
        u2 = 1.0;
    }
    if (u1 == 0.0 && u2 == 1.0) {
        glBegin(GL_LINES);
        sqsSendToRaster(v1);
        sqsSendToRaster(v2);
        glEnd();
        return;
    }

    sqsInterpolate(&v1, &v2, u1, u2);

    glBegin(GL_LINES);
    sqsSendToRaster(v1);
    sqsSendToRaster(v2);
    glEnd();
}

struct INTERNAL_VERTEX firstVertex[4], lastVertex[4];
char hasFirstVextex[4];

void sqsBeginTriangleClipping() {
    glBegin(GL_POLYGON);
    hasFirstVextex[0] = (char) 0;
    hasFirstVextex[1] = (char) 0;
    hasFirstVextex[2] = (char) 0;
    hasFirstVextex[3] = (char) 0;
}

void sqsClipTop(struct INTERNAL_VERTEX v) {
    if (!hasFirstVextex[3]) {
        hasFirstVextex[3] = (char) 1;
        sqsCloneVertex(&(firstVertex[3]), &v);
        if (!(v.clipcode & 4)) {
            sqsSendToRaster(v);
        }
    } else {
        char lastIn = !(lastVertex[3].clipcode & 4);
        char thisIn = !(v.clipcode & 4);
        if (lastIn) {
            if (thisIn) {
                sqsSendToRaster(v);
            } else { //de dentro para fora - modifica o segundo ponto
                double dy = v.normcoord[Y] - lastVertex[3].normcoord[Y];
                double dw = v.normcoord[W] - lastVertex[3].normcoord[W];
                if (dy - dw) {
                    double u = (-lastVertex[3].normcoord[Y] + lastVertex[3].normcoord[W]) / (dy - dw);
                    struct INTERNAL_VERTEX temp;
                    sqsCloneVertex(&temp, &v);
                    sqsInterpolate(&(lastVertex[3]), &temp, 0, u);
                    sqsSendToRaster(temp);
                }
            }
        } else {
            if (thisIn) { //de fora para dentro - modifica o primeiro ponto
                double dy = v.normcoord[Y] - lastVertex[3].normcoord[Y];
                double dw = v.normcoord[W] - lastVertex[3].normcoord[W];
                if (dy - dw) {
                    double u = (-lastVertex[3].normcoord[Y] + lastVertex[3].normcoord[W]) / (dy - dw);
                    struct INTERNAL_VERTEX temp;
                    sqsCloneVertex(&temp, &(lastVertex[3]));
                    sqsInterpolate(&temp, &v, u, 1);
                    sqsSendToRaster(temp);
                }
                sqsSendToRaster(v);
            }
        }
        if ((v.normcoord[X] == firstVertex[3].normcoord[X]) && (v.normcoord[Y] == firstVertex[3].normcoord[Y]) && (v.normcoord[Z] == firstVertex[3].normcoord[Z]) && (v.normcoord[W] == firstVertex[3].normcoord[W])) {
            hasFirstVextex[3] = (char) 0;
        }
    }
    sqsCloneVertex(&(lastVertex[3]), &v);
}

void sqsClipBottom(struct INTERNAL_VERTEX v) {
    if (!hasFirstVextex[2]) {
        hasFirstVextex[2] = (char) 1;
        sqsCloneVertex(&(firstVertex[2]), &v);
        if (!(v.clipcode & 8)) {
            sqsClipTop(v);
        }
    } else {
        char lastIn = !(lastVertex[2].clipcode & 8);
        char thisIn = !(v.clipcode & 8);
        if (lastIn) {
            if (thisIn) {
                sqsClipTop(v);
            } else { //de dentro para fora - modifica o segundo ponto
                double dy = v.normcoord[Y] - lastVertex[2].normcoord[Y];
                double dw = v.normcoord[W] - lastVertex[2].normcoord[W];
                if (dy + dw) {
                    double u = (lastVertex[2].normcoord[Y] + lastVertex[2].normcoord[W]) / -(dy + dw);
                    struct INTERNAL_VERTEX temp;
                    sqsCloneVertex(&temp, &v);
                    sqsInterpolate(&(lastVertex[2]), &temp, 0, u);
                    sqsClipTop(temp);
                }
            }
        } else {
            if (thisIn) { //de fora para dentro - modifica o primeiro ponto
                double dy = v.normcoord[Y] - lastVertex[2].normcoord[Y];
                double dw = v.normcoord[W] - lastVertex[2].normcoord[W];
                if (dy + dw) {
                    double u = (lastVertex[2].normcoord[Y] + lastVertex[2].normcoord[W]) / -(dy + dw);
                    struct INTERNAL_VERTEX temp;
                    sqsCloneVertex(&temp, &(lastVertex[2]));
                    sqsInterpolate(&temp, &v, u, 1);
                    sqsClipTop(temp);
                }
                sqsClipTop(v);
            }
        }
        if ((v.normcoord[X] == firstVertex[2].normcoord[X]) && (v.normcoord[Y] == firstVertex[2].normcoord[Y]) && (v.normcoord[Z] == firstVertex[2].normcoord[Z]) && (v.normcoord[W] == firstVertex[2].normcoord[W])) {
            hasFirstVextex[2] = (char) 0;
        }
    }
    sqsCloneVertex(&(lastVertex[2]), &v);
}

void sqsClipRight(struct INTERNAL_VERTEX v) {
    if (!hasFirstVextex[1]) {
        hasFirstVextex[1] = (char) 1;
        sqsCloneVertex(&(firstVertex[1]), &v);
        if (!(v.clipcode & 16)) {
            sqsClipBottom(v);
        }
    } else {
        char lastIn = !(lastVertex[1].clipcode & 16);
        char thisIn = !(v.clipcode & 16);
        if (lastIn) {
            if (thisIn) {
                sqsClipBottom(v);
            } else { //de dentro para fora - modifica o segundo ponto
                double dx = v.normcoord[X] - lastVertex[1].normcoord[X];
                double dw = v.normcoord[W] - lastVertex[1].normcoord[W];
                if (dx - dw) {
                    double u = (-lastVertex[1].normcoord[X] + lastVertex[1].normcoord[W]) / (dx - dw);
                    struct INTERNAL_VERTEX temp;
                    sqsCloneVertex(&temp, &v);
                    sqsInterpolate(&(lastVertex[1]), &temp, 0, u);
                    sqsClipBottom(temp);
                }
            }
        } else {
            if (thisIn) { //de fora para dentro - modifica o primeiro ponto
                double dx = v.normcoord[X] - lastVertex[1].normcoord[X];
                double dw = v.normcoord[W] - lastVertex[1].normcoord[W];
                if (dx - dw) {
                    double u = (-lastVertex[1].normcoord[X] + lastVertex[1].normcoord[W]) / (dx - dw);
                    struct INTERNAL_VERTEX temp;
                    sqsCloneVertex(&temp, &(lastVertex[1]));
                    sqsInterpolate(&temp, &v, u, 1);
                    sqsClipBottom(temp);
                }
                sqsClipBottom(v);
            }
        }
        if ((v.normcoord[X] == firstVertex[1].normcoord[X]) && (v.normcoord[Y] == firstVertex[1].normcoord[Y]) && (v.normcoord[Z] == firstVertex[1].normcoord[Z]) && (v.normcoord[W] == firstVertex[1].normcoord[W])) {
            hasFirstVextex[1] = (char) 0;
        }
    }
    sqsCloneVertex(&(lastVertex[1]), &v);
}

void sqsClipLeft(struct INTERNAL_VERTEX v) {
    if (!hasFirstVextex[0]) {
        hasFirstVextex[0] = (char) 1;
        sqsCloneVertex(&(firstVertex[0]), &v);
        if (!(v.clipcode & 32)) {
            sqsClipRight(v);
        }
    } else {
        char lastIn = !(lastVertex[0].clipcode & 32);
        char thisIn = !(v.clipcode & 32);
        if (lastIn) {
            if (thisIn) {
                sqsClipRight(v);
            } else { //de dentro para fora - modifica o segundo ponto
                double dx = v.normcoord[X] - lastVertex[0].normcoord[X];
                double dw = v.normcoord[W] - lastVertex[0].normcoord[W];
                if (dx + dw) {
                    double u = (lastVertex[0].normcoord[X] + lastVertex[0].normcoord[W]) / -(dx + dw);
                    struct INTERNAL_VERTEX temp;
                    sqsCloneVertex(&temp, &v);
                    sqsInterpolate(&(lastVertex[0]), &temp, 0, u);
                    sqsClipRight(temp);
                }
            }
        } else {
            if (thisIn) { //de fora para dentro - modifica o primeiro ponto
                double dx = v.normcoord[X] - lastVertex[0].normcoord[X];
                double dw = v.normcoord[W] - lastVertex[0].normcoord[W];
                if (dx + dw) {
                    double u = (lastVertex[0].normcoord[X] + lastVertex[0].normcoord[W]) / -(dx + dw);
                    struct INTERNAL_VERTEX temp;
                    sqsCloneVertex(&temp, &(lastVertex[0]));
                    sqsInterpolate(&temp, &v, u, 1);
                    sqsClipRight(temp);
                }
                sqsClipRight(v);
            }
        }
        if ((v.normcoord[X] == firstVertex[0].normcoord[X]) && (v.normcoord[Y] == firstVertex[0].normcoord[Y]) && (v.normcoord[Z] == firstVertex[0].normcoord[Z]) && (v.normcoord[W] == firstVertex[0].normcoord[W])) {
            hasFirstVextex[0] = (char) 0;
        }
    }
    sqsCloneVertex(&(lastVertex[0]), &v);
}

void sqsEndTriangleClipping() {
    if (hasFirstVextex[0]) {
        sqsClipLeft(firstVertex[0]);
    }
    if (hasFirstVextex[1]) {
        sqsClipRight(firstVertex[1]);
    }
    if (hasFirstVextex[2]) {
        sqsClipBottom(firstVertex[2]);
    }
    if (hasFirstVextex[3]) {
        sqsClipTop(firstVertex[3]);
    }
    glEnd();
}

void sqsClipTriangle(struct INTERNAL_VERTEX v1, struct INTERNAL_VERTEX v2, struct INTERNAL_VERTEX v3) {
    if ((v1.clipcode | v2.clipcode | v3.clipcode) == 0) {
        glBegin(GL_TRIANGLES);
        sqsSendToRaster(v1);
        sqsSendToRaster(v2);
        sqsSendToRaster(v3);
        glEnd();
        return;
    }
    if ((v1.clipcode & v2.clipcode & v3.clipcode) != 0) {
        return;
    }
    sqsBeginTriangleClipping();
    sqsClipLeft(v1);
    sqsClipLeft(v2);
    sqsClipLeft(v3);
    sqsEndTriangleClipping();
}
